/*******************************************************************************
* This file implements the PID control system to control the roll, pitch and
* yaw of the QuadCopter.
*
* Author: Kong Wai Weng @ Cytron Technologies Sdn. Bhd.
*******************************************************************************/



#include "control.h"



/*******************************************************************************
* PRIVATE CONSTANT DEFINITION                                                  *
*******************************************************************************/

// Period for the control loop (5ms - 200Hz).
#define PID_DT			0.005f



/*******************************************************************************
* PRIVATE GLOBAL VARIABLES                                                     *
*******************************************************************************/

// Error sum of the PID controllers.
static volatile float fRollErrorSum = 0.0;
static volatile float fPitchErrorSum = 0.0;
static volatile float fNavErrorSum = 0.0;
static volatile float fAirspeedErrorSum = 0.0;
static volatile float fTotalEnergyErrorSum = 0.0;

// Stick offset.
static unsigned int uiAileronOffset = PULSE_WIDTH_1500;
static unsigned int uiElevatorOffset = PULSE_WIDTH_1500;
static unsigned int uiRudderOffset = PULSE_WIDTH_1500;



/*******************************************************************************
* PRIVATE FUNCTION PROTOTYPES                                                  *
*******************************************************************************/

static void prv_vControlRoll(float fSetpoint, float fRoll, float fRollRate);
static void prv_vControlPitch(float fSetpoint, float fPitch, float fPitchRate);
static float prv_fControlHeading(float fSetpoint, float fHeading);
static float prv_fControlAirspeed(float fSetpoint, float fAirspeed);



/*******************************************************************************
* TASK: taskControlLoop
*
* DESCRIPTIONS:
* This task is the PID control loop and will get executed every 5ms (200Hz).
*
*******************************************************************************/
portTASK_FUNCTION(taskControlLoop, pvParameters)
{	
//	// Wait until the system is ready.
//	while ((xSystemState.bDcmReady == 0) ||
//		   (xSystemState.bAltitudeReady == 0) ||
//		   (xSystemState.bNavigationReady == 0)) {
//		// Let other task to run while waiting.
//		// We check every 200ms.
//		vTaskDelay(configTICK_RATE_HZ / 5);
//	}
	
	
	
	// Indicate the control loop is ready.
	xSystemState.bControlReady = 1;
	
	
	
	// We need to initialize xLastFlashTime prior to the first call to vTaskDelayUntil().
	portTickType xLastFlashTime = xTaskGetTickCount();
	
	while (1) {
		struct ATTITUDE xAttitude = xGetAttitude();
		struct GPS_DATA xGpsData = xGetGpsData();

//                xGpsData.fGroundSpeed = 15.0f;
		
		// If it is manual mode and there is valid pulse from the RC Rx...
		if ((xSystemError.bRcRxError == 0) && (eGetControlMode() == MANUAL)) {
			// Record the current stick position as the offset.
			uiAileronOffset = uiGetRcRx(AILERON_CHANNEL);
			uiElevatorOffset = uiGetRcRx(ELEVATOR_CHANNEL);
			uiRudderOffset = uiGetRcRx(RUDDER_CHANNEL);
		}	
		
		// If it is auto mode or the Rx signal is lost...
		else {
			// Navigate by controlling the roll angle.
			float fHeadingSetpoint = fGetHeadingSetpoint();

//			float fRequiredTurnRate = prv_fControlHeading(fHeadingSetpoint, xGpsData.fGroundCourse * DEG_TO_RAD);
			float fRequiredTurnRate = prv_fControlHeading(fHeadingSetpoint, xAttitude.fYaw);
//                        float fRequiredTurnRate = ((float)uiAileronOffset - (float)uiGetRcRx(AILERON_CHANNEL)) / 2500.0f * 50.0f * DEG_TO_RAD;
			float fRequiredRoll = atanf(xGpsData.fGroundSpeed * fRequiredTurnRate / GRAVITY);
			fRequiredRoll = LIMIT(fRequiredRoll, -MAX_ROLL, MAX_ROLL);
			prv_vControlRoll(ROLL_OFFSET + fRequiredRoll, xAttitude.fRoll, xAttitude.fRateX);
			
			
			
//			// Control airspeed by pitch angle (35km/h).
//			float fRequiredPitch = prv_fControlAirspeed(35.0f / MS_TO_KMH, fGetAirspeed());
//			
//			// Calculate the pitch offset for roll compensation.
//			float fPitchOffset = fabsf(xAttitude.fRoll * PITCH_ROLL_COMP);

                        float fRequiredPitch = ((float)uiElevatorOffset - (float)uiGetRcRx(ELEVATOR_CHANNEL)) / 2500.0f * 30.0f * DEG_TO_RAD;
			
//			// Control pitch angle.
//			prv_vControlPitch(PITCH_OFFSET + fRequiredPitch + fPitchOffset, xAttitude.fPitch, xAttitude.fRateY);
			prv_vControlPitch(PITCH_OFFSET + fRequiredPitch, xAttitude.fPitch, xAttitude.fRateY);
			
			// Make sure the pulse is valid.
			if (xSystemError.bRcRxError == 0) {
				vUpdateServo(THROTTLE_CHANNEL, uiGetRcRx(THROTTLE_CHANNEL));
//				vUpdateServo(ELEVATOR_CHANNEL, uiGetRcRx(ELEVATOR_CHANNEL));
			}	
		}		
		
		
		
		// Record the stack high water mark.
		xStackWaterMark.uiControlLoop = uxTaskGetStackHighWaterMark(NULL);
		
		// Set the error flag if the water mark is too low.
		if (xStackWaterMark.uiControlLoop < RTOS_STACK_THRESHOLD) {
			xSystemError.bRtosStackError = 1;
		}
		
		// Loop every 5ms (200 Hz).
		vTaskDelayUntil(&xLastFlashTime, configTICK_RATE_HZ / 200);
	}	
}	



/*******************************************************************************
* PRIVATE FUNCTION: prv_vControlRoll
*
* PARAMETERS:
* ~ fSetpoint	- The desired roll angle(rad).
* ~ fRoll		- The actual roll angle (rad).
* ~ fRollRate	- The actual roll rate (rad/s).
*
* RETURN:
* ~ void
*
* DESCRIPTION:
* Control the roll angle by adjusting the aileron.
*
*******************************************************************************/
static void prv_vControlRoll(float fSetpoint, float fRoll, float fRollRate)
{
	static float fOutput = 0.0f;
	
	
	
	// Calculate the error and offset the error to within +-PI.
	float fError = fSetpoint - fRoll;
	CONVERT_NEGPI_PI(fError);
	
	
	
	// Only accumulate the error when the output is not saturated.
	if (((fOutput < 0.25f) || (fError < 0.0f)) && 
		((fOutput > -0.25f) || (fError > 0.0f))) {
		
		fRollErrorSum += fError * PID_DT;
	}
	
	
	
	// Calculate the proportional, integral and derivative term.
	float fPTerm = xSystemConfig.fRollKp * fError;
	float fITerm = xSystemConfig.fRollKi * fRollErrorSum;
	float fDTerm = xSystemConfig.fRollKd * -fRollRate;
	
	// Calculate the output.
	fOutput = LIMIT(fPTerm + fITerm + fDTerm, -1.0f, 1.0f);
	
	// Calculate the aileron pulse width.
	signed int siAileronOut = (signed int)uiAileronOffset +
                                  ((signed int)(fOutput * (float)PULSE_WIDTH_500) * AILERON_DIR);
	
	// Calculate the rudder pulse width.
	signed int siRudderOut = (signed int)uiRudderOffset +
                                 ((signed int)(fOutput * (float)PULSE_WIDTH_500 * xSystemConfig.fAilRudRatio) * RUDDER_DIR);
	
	// Update the aileron servo.
	vUpdateServo(AILERON_CHANNEL, (unsigned int)siAileronOut);
	vUpdateServo(LEFT_AILERON_CHANNEL, (unsigned int)siAileronOut);
	vUpdateServo(RIGHT_AILERON_CHANNEL, (unsigned int)siAileronOut);

	// Update the rudder servo.
	vUpdateServo(RUDDER_CHANNEL, (unsigned int)siRudderOut);
}



/*******************************************************************************
* PRIVATE FUNCTION: prv_vControlPitch
*
* PARAMETERS:
* ~ fSetpoint	- The desired pitch angle(rad).
* ~ fPitch		- The actual pitch angle (rad).
* ~ fPitchRate	- The actual pitch rate (rad/s).
*
* RETURN:
* ~ void
*
* DESCRIPTION:
* Control the pitch angle by adjusting the elevator.
*
*******************************************************************************/
static void prv_vControlPitch(float fSetpoint, float fPitch, float fPitchRate)
{
	static float fOutput = 0.0f;
	
	
	
	// Calculate the error and offset the error to within +-PI.
	float fError = fSetpoint - fPitch;
	CONVERT_NEGPI_PI(fError);
	
	
	
	// Only accumulate the error when the output is not saturated.
	if (((fOutput < 0.25f) || (fError < 0.0f)) && 
		((fOutput > -0.25f) || (fError > 0.0f))) {
		
		fPitchErrorSum += fError * PID_DT;
	}
	
	
	
	// Calculate the proportional, integral and derivative term.
	float fPTerm = xSystemConfig.fPitchKp * fError;
	float fITerm = xSystemConfig.fPitchKi * fPitchErrorSum;
	float fDTerm = xSystemConfig.fPitchKd * -fPitchRate;
	
	// Calculate the output.
	fOutput = LIMIT(fPTerm + fITerm + fDTerm, -1.0f, 1.0f);
	
	// Calculate the elevator pulse width.
	signed int siElevatorOut = (signed int)uiElevatorOffset +
                                   ((signed int)(fOutput * (float)PULSE_WIDTH_500) * ELEVATOR_DIR);
	
	// Update the servo.
	vUpdateServo(ELEVATOR_CHANNEL, (unsigned int)siElevatorOut);
}



/*******************************************************************************
* PRIVATE FUNCTION: prv_fControlHeading
*
* PARAMETERS:
* ~ fSetpoint	- The desired heading of the UAV (0 - 2PI rad).
* ~ fHeading	- The actual heading of the UAV (0 - 2PI rad).
*
* RETURN:
* ~ The turn rate setpoint (rad/s).
*
* DESCRIPTION:
* Control the heading of the UAV.
*
*******************************************************************************/
static float prv_fControlHeading(float fSetpoint, float fHeading)
{
	static float fOutput = 0.0f;
	
	
	
	// Calculate the error and offset the error to within +-PI.
	float fError = fSetpoint - fHeading;
	CONVERT_NEGPI_PI(fError);
	
	
	
	// Only accumulate the error when the output is not saturated.
	if (((fOutput < (MAX_TURN_RATE / 4.0f)) || (fError < 0.0f)) && 
		((fOutput > -(MAX_TURN_RATE / 4.0f)) || (fError > 0.0f))) {
		
		fNavErrorSum += fError * PID_DT;
	}
	
	
	
	// Calculate the proportional and integral term.
	float fPTerm = xSystemConfig.fNavKp * fError;
	float fITerm = xSystemConfig.fNavKi * fNavErrorSum;
	
	// Calculate and limit the output to maximum turn rate.
	// The output is the turn rate (rad/s).
	fOutput = LIMIT(fPTerm + fITerm, -MAX_TURN_RATE, MAX_TURN_RATE);
	
	// Return the value.
	return fOutput;
}



/*******************************************************************************
* PRIVATE FUNCTION: prv_fControlAirspeed
*
* PARAMETERS:
* ~ fSetpoint	- The desired airspeed of the UAV (knots).
* ~ fAirspeed	- The current airspeed of the UAV (knots).
*
* RETURN:
* ~ The pitch setpoint of the UAV (rad).
*
* DESCRIPTION:
* Control the airspeed of the UAV by changing the pitch angle.
*
*******************************************************************************/
static float prv_fControlAirspeed(float fSetpoint, float fAirspeed)
{
	static float fOutput = 0.0f;
	
	// Calculate the error.
	float fError = fSetpoint - fAirspeed;
	
	// Only accumulate the error when the output is not saturated.
	if (((fOutput < (MAX_POS_PITCH / 4.0f)) || (fError < 0.0f)) && 
		((fOutput > -(MAX_NEG_PITCH / 4.0f)) || (fError > 0.0f))) {
		
		fAirspeedErrorSum += fError * PID_DT;
	}
	
	
	
	// Calculate the proportional term.
	float fPTerm = xSystemConfig.fAirspeedKp * fError;
	
	// Calculate the integral term.
	float fITerm = xSystemConfig.fAirspeedKi * fAirspeedErrorSum;
	
	// Calculate and limit the output.
	// The output is the inversed pitch angle (rad).
	fOutput = LIMIT(-(fPTerm + fITerm), -MAX_NEG_PITCH, MAX_POS_PITCH);
	
	// Return the value.
	return fOutput;
}	
